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ABSTRACT 

We  propose  a  3D  self-localisation  method  that  uses  3D  angle  of  arrival  (AOA)  information  (ie., 
azimuth  and  elevation  measurements)  from  landmarks.  The  formulation  is  based  on 
minimising  the  collinearity  error  between  the  estimated  line  of  sight  (LOS)  to  the  landmark 
and  the  measured  AOA.  This  method  runs  in  two  parts  -  initial  estimation  of  the  vehicle 
azimuth  and  position  assuming  the  vehicle  has  no  tilt,  and  iterative  3D  pose  estimation  based 
on  a  small  angle  approximation  approach.  Simulation  study  indicates  that  this  method  is 
efficient  requiring  a  small  number  of  iterations,  globally  convergent  and  robust. 
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3D  Self-Localisation  from  Angle  of  Arrival 
Measurements 


Executive  Summary 

In  this  report,  we  introduce  a  3D  self-localisation  method  that  uses  three-dimensional 
angle  of  arrival  (AOA)  information  (i.e.  azimuth  and  elevation  measurements)  from 
landmarks.  We  assume  here  that  such  measurements  are  available,  even  though  in 
practice  they  need  to  be  processed  from  the  onboard  sensors  such  as  cameras  or  RF 
receivers. 

This  work  is  a  follow  on  to  our  previous  study  on  2D  localisation  where  the  altitude 
and  the  tilt  angle  of  the  vehicle  were  not  relevant.  However,  in  many  applications  the 
vehicle  on-board  sensor  may  be  tilted  making  bearing  measurements  erroneous.  In 
these  situations  the  localisation  problem  has  to  be  formulated  in  six-degrees  of  freedom 
(i.e.  3  DOF  for  sensor  position  and  3  DOF  for  sensor  orientation). 

The  formulation  is  based  on  minimising  the  collinearity  error  between  the  estimated 
line  of  sight  (LOS)  and  the  measured  AOA.  Using  such  error  metric,  we  arrive  at  an 
iterative  algorithm  that  runs  in  two  parts:  initial  estimation  of  position  and  azimuth 
assuming  zero  tilt,  followed  by  iterative  orientation  and  position  updates  using  small 
angle  approximation  approach. 

The  proposed  method  is  evaluated  against  a  benchmark  known  as  the  orthogonal 
iteration  which  is  known  to  be  accurate,  globally  convergent,  and  efficient.  The 
experimental  results  indicate  that  the  proposed  method  is  more  accurate  than  the 
benchmark.  The  proposed  method  is  also  efficient  requiring  a  small  number  of 
iterations  and  appears  to  be  globally  convergent. 
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1.  Introduction 


The  capabilities  that  unmanned  robotic  vehicles  provide  are  expected  to  revolutionise  the  way 
combat  operations  are  conducted.  The  UAV  or  UGV  will  reduce  the  risk  to  soldiers  in 
hazardous  situations,  and  alleviate  the  human  workload  and  manpower  requirements 
Possible  applications  include:  search  and  rescue  (in  land  and  sea),  surveillance  of  military 
bases  or  nuclear  sites,  demining  activities,  underwater  construction  and  mapping,  agriculture, 
logistics  in  battle  fields. 

One  of  the  key  components  in  autonomous  functionality  is  localisation,  which  is  the  process  of 
determining  its  own  position  and  orientation  using  onboard  sensors  or  radio  links.  We  are 
particularly  interested  in  landmark-based  localisation  which  is  attractive  in  a  controlled 
environment  where  the  landmarks  can  be  precisely  located. 

In  open  outdoor  environments,  differential  GPS  systems  can  provide  precise  position 
information.  However,  there  are  situations  where  GPS  is  not  adequate  such  as  indoor, 
underwater,  extraterrestrial  or  urban  environments,  because  of  signal  blockage  and  multipath 
interferences.  Furthermore,  for  stand-alone  GPS  systems,  the  position  accuracy  may  not  be 
sufficient  In  military  context,  GPS  receivers  at  low  altitudes  are  more  susceptible  to  jamming 
or  spoofing  from  the  adversary.  A  simple  alternative  would  be  odometry  or  inertial 
navigation  system  (INS).  However,  both  are  subject  to  build-up  of  error  with  time,  and  often 
require  an  external  aid. 

In  this  report,  we  introduce  a  3D  self-localisation  method  that  uses  3D  angle  of  arrival  (AO A) 
measurements  (i.e.  azimuth  and  elevation)  from  surrounding  landmarks.  We  assume  that  the 
AOA  measurements  are  available,  even  though  in  practice  they  need  to  be  collected  from  the 
onboard  sensors  such  as  cameras  or  RF  receivers. 

This  work  is  an  extension  of  our  previous  report  on  2D  localisation  [4]  where  the  altitude  and 
the  tilt  angle  were  not  considered  (assumed  zero).  In  practice,  vehicles  (especially  the  airborne 
vehicles)  can  be  tilted  at  times,  adding  errors  to  the  bearing-only  measurements.  In  these 
situations  the  localisation  problem  has  to  be  formulated  in  six-degrees  of  freedom  (i.e.  3  DOF 
for  sensor  position  and  3  DOF  for  sensor  orientation). 

We  formulate  the  localisation  problem  as  one  of  minimising  the  collinearity  error  between  the 
estimated  line  of  sight  (LOS)  and  the  measured  AOA,  as  shown  in  Figure  1.  Using  such  error 
metric,  we  arrive  at  an  iterative  algorithm  that  runs  in  two  parts:  initial  estimation  of  position 
and  azimuth  assuming  zero  tilt,  followed  by  iterative  orientation  and  position  updates  using 
small  angle  approximation  approach.  This  method  is  efficient  requiring  a  small  number  of 
iterations  and  is  shown  to  be  globally  convergent  and  robust. 
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Figure  1:  Illustration  ofUAV  taking  noisy  AO  A  measurements  from  landmarks  in  order  to  estimate 
its  position  and  orientation 


This  report  is  organised  as  follows:  Section  2  describes  the  formulation  of  the  proposed 
method  and  re-visits  an  existing  method  which  will  be  used  as  a  benchmark.  In  Section  3, 
simulation  results  and  performance  comparisons  with  the  benchmark  are  given.  Finally, 
Section  4  summaries  and  concludes  the  study. 


2.  Problem  Formulations 

2.1  Optimisation  of  Translation  Vector 

The  points,  p, ,  in  Figure  1,  are  landmark  positions  given  in  the  external  reference  frame.  We 
first  move  them  so  that  their  mean  coincides  with  the  new  origin.  The  translated  landmark 
positions  are  given  by 

q,  =p(-p  where  p  =  -^p  .  (1) 

Note  that  q  =  0, 

In  the  translated  frame,  we  define  b  as  the  vehicle  position,  and  v.  as  the  unit  line  of  sight 
(uLOS)  vector  from  b  to  q, ,  expressed  in  body  frame  (see  Figure  2). 
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In  absence  of  noise 
R(q.-b) 


v  =  - 


d, 

+  * 


where  di=||ql-b|| 

t  =  -Rb 


R  :  Rotation  from  Reference 
Frame  to  Body  Frame 


<  .r 

'  Reference  Frame  r 


Body  frame 


Figure  2:  Definition  of  unit  line  of  sight  vector  v,  and  translation  vector  t.  Note  v  is  expressed  in  the 
sensor  (body)  frame. 

In  the  absence  of  measurement  errors,  we  have  the  following  relationship, 

R(q, -b)  Rq, +t 

'  d,  d,  W 

where 

•  d}  is  the  distance  between  q(  and  the  estimated  vehicle  position,  b  . 

•  R  is  the  rotation  matrix  from  the  reference  frame  to  the  body  frame,  and 

•  t  is  the  translation  vector  (t  =  -Rb) . 

In  the  presence  of  measurement  errors,  v;is  no  longer  equal  to  ~ /  and  the  objective 
becomes  to  minimise  the  following  cost  function. 


V  V 

Rq,+t 

1  ' 

L  4  J 

(3) 


where  v,  is  the  unit  pointing  vector  along  the  measured  azimuth  jdt  and  elevation  at  to  the  ith 
landmark.  This  error  metric  is  a  measure  of  the  collinearity  between  the  measured  and 
estimated  uLOS  vectors. 
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Rq,  +  t 


Figure  3:  Definition  of  the  error  metric  as  the  collineanty  of  estimated  and  measured  uLOS  vectors 


Examining  Figure  3,  the  error  given  by  (3)  can  also  be  expressed  as 


£(R,t)  =  X 


,vM 


d: 


y 
V  L 


Rq.  +t 


(4) 


If  we  let  Vj  =  v(v;  ,  then  the  optimal  translation  vector,  t,  can  be  obtained  by  solving  the 
equation  below. 

V,£(R,t)  =  £^{2(1  -V,)t  +  2(1  -  V,)Rq, }  =  0  (5) 

i=l 

If  we  let 
( I  -  V, ) 

A,  =L-jrd'  (3x3)  (6) 


Then  (5)  becomes 


(lA,)‘  +  LA,Rq,=0 

i*l  »=l 

By  rearranging  (7),  we  obtain  topl  as  below, 

top,  =  “A  'X A/Rq,  where  A  =  Ja,  . 

/=!  /«! 


(7) 


(8) 
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2.2  Azimuth  and  Position  Estimation  (AZIPE) 

The  rotation  terms  (ie.,  roll,  pitch,  and  azimuth)  are  expressed  as  sums  and  products  of 
trigonometric  functions  in  R ,  making  the  objective  function  in  Equation  (4)  highly  non-linear. 
Finding  the  closed-form  solution  for  all  three  angles  is  very  challenging. 

To  simplify  the  approach,  we  assume  that  the  roll  ( (j> )  and  pitch  (#)  are  known,  and  try  to  find 
the  optimal  azimuth  (y/) .  The  rotation  from  the  reference  to  body  frames  is  given  as 


R  = 


hcosy/ 

kcosy/  -Ismy/ 
ucosy/  +  vsiny/ 


hsiny/  -a 

l  cosy/  +  £siny/  b 
-vcosy/  +  us\x\y/  c 


where 

h  =  cos#,  k  =  sin <j> sin#,  /  =  cos^,  u  =  cos ^ sin# 
v  =  sin^,  a  =  sin#,  6  =  sin ^ cos#,  c  =  cos^cos# 

We  define  e  the  vector  to  be  estimated  as 

e  =  [cos^,siny/] 


(9) 


(10) 


Then  Rq;  in  Equation  (8)  can  be  expressed  as  linear  equation  of  e. 


Rq,  =Q,e  +  s,. 


where  q,  = 

’<7,/ 

% 

>  Q,  = 

Hx 

kQix+1% 

hqly 

-lqu+kqiy 

and  s,  =  qu 

-a 

b 

Jh,_ 

uqix-vqly 

vqtx+Wty  _ 

c 

Therefore  iopt  in  (8)  becomes 

Me)  =  -A"'ZA.(Q,e+s,) 

i=\ 

=[-a-'±a,qX-a-'±a/S: 

v  mi  y  1=1 


Me)  =  Fe  +  w 

n  n 

where  F  =  -A~']T  A,Q,  and  w  =  -A~'^A,s, 


Finally,  from  (11)  and  (13),  we  have  an  expression  for  Rq  +t  as 
Rq,  +  V  =G,e+gl 


(11) 


(12) 


(13) 

(14) 


(15) 
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where  G,  =  F  +  Q,  and  g(.  =  w  +  s,. . 


(16) 


After,  substituting  (16)  into  (4)  (see  Appendix  B  for  details),  we  arrive  at  the  following 
quadratic  cost  function 


£(e)  =  e'  Me  +  2nT  t  +  d 
where 

M  =  SGfA,G(  (2x2) 
m^G^g,  (2x1)  and 

i=\ 

^  =  Zg,'A,g, 


(17) 

(18) 

(19) 

(20) 


We  substitute  (10)  into  (17)  and  obtain, 


£(y/)  =  [  cosy/  sin  <//] 


X 

a/I2' 

cosy/ 

X 

X. 

siny/ 

+  2/w,  cosy/  +  2m2  sin  y/  +  d 

=  (A/„  -  A/22)cos2  y/  +  2Mn  cos y/ sin y/  +  2/w,  cosy/  +2/w2  siny/  +  A/22  +  d 
=  ,4  cos2  if/  +  5  cos  if/  sin  if/  4-  C  cos  if/  +  S  sin  if/  +  D . 


(21) 


The  optimal  angle  is  obtained  by  differentiating  E  with  respect  to  if/  and  setting  the 
derivative  to  zero  as. 


dE  .  2 

- =  -2  A  cos  if/  sin  if/  +  2  B  cos  if/  -  B  -  C  sin  if/  +  S  cos  if/  -  0 

dy/ 

This  leads  to 

2  B  cos2  if/  +  S  cos  if/  -  B  =  2^  cos  if/  sin  if/  +  C  sin  if/  q 
which  after  squaring  and  rearranging,  results  in 


A.x4  +  Tr3  +  A7x2  +  A.x  +  Aa  —  0 


(22) 


(23) 


(24) 


where  x  =  cosy/  and 


At=4(A2+B2) 
A3=4(AC  +  BS) 

a2=s2+c2-a4 

A ,  =  -4AC -IBS 
A0  =  B1-C 2 
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Solving  this  quartic  equation  may,  in  theory,  yield  up  to  4  real  solutions.  However,  only  those 
satisfying  -1<x<1  are  acceptable.  The  azimuth  is  given  as  =  cos’1  (x)  for  each  solution  x, 
and  we  choose  the  one  associated  with  the  smallest  E{y/) . 


We  put  y/  into  Equation  (9)  to  obtain  the  optimal  rotation,  R ,  and  the  vehicle  position  is 
then  obtained  as 

Pv^—R'top+P-  (25) 

This  solution  is  accurate  if  the  initial  tilt  angles  are  known  or  accurately  guessed.  Often  the 
exact  tilt  angles  are  unknown  but  small.  We  therefore  assume  they  are  zero,  solve  (24),  and 
obtain  an  approximate  solution  for  the  vehicle's  position  and  orientation.  This  approximate 
solution  can  be  refined  as  explained  in  the  next  subsection.  But  before  moving  to  the  next 
subsection,  we  note  that  once  an  estimated  vehicle  position  is  computed,  then  approximate 
values  for  the  parameters,  di ,  in  (2)  can  be  obtained  as  |pvehicle  -p,||. 


2.3  Angle  Increments  and  Position  Estimation  (AIPE) 

Once  the  AZIPE  algorithm  is  executed,  we  are  given  initial  orientation  estimates  for  ^n,  60 
and  y/Q .  Often  these  estimates  are  very  close  to  the  optimal  solution.  To  refine  their  estimates, 
we  perturb  them  as  <j>  -  <j>0  +  8<f> ,  0  =  60+86 ,  and  y/  =  y/0  +  Sy/ ,  and  re-derive  the  objective 
function  (4)  using  the  new  small  angle  variables,  (8<f>,86y8y/) . 


The  rotation  matrix  by  definition  is  given  as 


R  = 


cos#cosy/ 

-cos^siny/  +  sin0sin#cosy/ 
sin  $  sin  y/  +  cos^  cosy/ sin# 


cos#siny/  -sin# 

cos  ^  cosy/  +  sin  0  sin#  sin  y/  cos  #  sin  (p 

— cosy/  sin  </>  +  sin#  sin  y/cos^  cos^cos# 


(26) 


By  substituting  (f>  =  <f>0  +  5</> ,  6  -  0Q  +  86 ,  and  y/  =  y/0  +  Sy/  into  Equation  (26),  expanding  and 
collecting  the  first  order  terms,  then  each  element  in  R  can  be  expressed  as  a  linear  function  of 
the  incremental  terms  {8(j),86,8y/} . 


cl  c2  c  3 

R  =  c4  c5  c6  (27) 

cl  c8  c9 

where  ci  =  c/0  +  cixS<j>  +  ci286  +  ci3Sy/  J  =  1 . .  .9 .  The  details  of  elements  (cl, . . .,  c9)  are  given  in 
Appendix  B 


Again  we  need  to  re-express  Rq,  from  Equation  (8)  in  the  form  of  Qfe  +  s, . 


cl 

c  2 

c3~ 

<hx 

Rq,= 

c4 

c5 

c6 

% 

_cl 

c8 

c9 

_<hz_ 
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(cl0?„  +c20q,y  +  c30?„)  +  (cl,9„  +c2,qv  +  c3lqa)A<f>  +  (c\2qn  +  c22qly  +  c32qa)A0 +  (cl,qix  +  c2,qty  +c3,qit)Ai// 

=  (c40?«  +  cio%  +c60?,,)  +  (c4i9„  +c5i%+c6lqa)Ai/i  +  (c‘\2q,x  +  c52q,y  +  c62qlx)A0  +  (c4iq,x  +c5,q,y  +  c6,qlx)Ai// 

(c7o?«  +c8o<7„  +  c9o‘7«)  +  (c7i9*  +c%,%  +c9|<7»)A^  +  (c729i,  +  ci2qly+c92qix)A0  +  (cl,qtx  +ci,q,v  +c9,q„)Ai//_ 

=  Q,e  +  s(  (28) 


where 


0,  = 


c\qt,*c2iqlr+cilqa 
c4l9„+c5l‘7,y+c6l9,r 
57,9a +c8|^  +  c9,9tt 


C,2  9„+C22^+C329tt 
c42‘7„  +  ‘:52‘7,v+c62‘7,; 
c72qix  +  c&2qiv+c92qit 


c,j<7<,+c2j4V+c3j4* 
c4j^tt  +c5Jg(y  +  c6,9„ 
cl,q„+ci,qly  +  c9,q„ 


e  = 


s<t> 

89 


8y/ 


s.  = 


cl0&r+c20^+c30^ 

c4o?„+c50^+c60^ 

c70^+c80^+c90<7ft 


(29) 


(30) 


(31) 


Having  derived  Q, ,  e,  and  s,,  we  obtain  a  new  objective  function  similar  to  (17). 

£(e)  =  e?  Me  +  2m7  e  +  d  (32) 

where  M=^GfA,G(/  m  =  ^G,7  A,g,  and  d  =  £g,rA(g,  (33) 


Differentiating  (32)  with  respect  to  e  and  setting  it  to  zero  yields 

Ve£  =  2Me  +  2m  =  0  (34) 

Rearranging  Equation  (34),  we  obtain  the  optimal  solution 

eop=-M-1m  (35) 

Once  e  =  [8$,89,yy/]7  is  computed,  we  have  the  improved  angle  estimates. 


1 

& 

'  sf 

** 

= 

00 

+ 

se 

y°p. 

St// 

(36) 


We  construct  R  from  these  updated  Euler  angles,  and  compute  the  vehicle  position  as 

Probo,  =-R,‘op+P  (37) 

The  angle  updates  (35)  and  (36)  are  usually  applied  several  times  until  no  significant  change 
occurs  in  e.  In  our  applications,  we  found  that  no  more  than  two  iterations  of  (35)  and  (36)  are 
needed  to  obtain  the  optimal  solution. 


8 


DSTO-TR-2278 


2.4  Orthogonal  Iteration  (OI)  Algorithm  as  Baseline 

In  this  subsection,  we  present  a  pose  estimation  method  known  as  the  orthogonal  iteration  (OI), 
proposed  by  [6].  This  method  is  also  based  on  the  collinearity  property,  and  is  shown  to  be 
efficient  and  robust.  We  will  describe  the  formulation  of  this  method,  as  it  was  used  as  a 
benchmark  for  performance  comparison 


The  objective  is  to  minimise  the  following  cost  function 

£(r.  t)=ij 


||  Rq,  +t 

2 

f 

Rq,+t"| 

I  d, 

V/ 

\ 

L  d,  JJ 

(38) 


which  is  identical  to  our  cost  function  in  Equation  (2).  The  optimal  translation  vector,  t,  is  also 
obtained  as 


KP,  =-A'‘XA-R(i/ where  A  =  ZA;  and  A-  (39) 

i=  l  /=1  «| 

Note  that  obtaining  the  actual  value  for  topt  requires  the  rotation  matrix  R  .  This  R  can  be 
obtained  from  the  following  algorithm  known  as  the  absolute  orientation  [1-2]. 

2.5  Absolute  Orientation  (AO) 

Assuming  that  we  have  landmark  position  coordinates  in  both  the  body  and  reference  frames, 
the  absolute  orientation  algorithm  finds  topf  and  R  which  minimise  the  least-square  error 

problem  defined  as  below. 

n 

Minimise  £(R,t)  =  ^||Rq,  +t-rj2  subject  to  RrR  =  I  (40) 


where  r;  =  dj\l . 

Differentiating  (40)  with  respect  to  t,  setting  it  to  zero,  and  noting  q  =  0  yields 

KP,  =  -  Z(-Rqf  +0  =  -Rq  +7=7.  (41) 

n  ,=i 

Substituting  (41)  into  (40)  yields 
£(R,t)  =  J||Rq,  -r,f  where  r/  =  r. -1;. 

i=l 

=  Z(q'  q,  +  r/7  r/-2r/rRqj ) .  (42) 

/=! 
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Our  optimisation  problem  becomes  maximising  Rq,  subject  to  R  R  =  I  Since 

/=! 

^r/;  Rq,  is  scalar,  then  £r/rRq,  =  "'(X>‘,'rRq/) 

i=\  i= 1  /=! 

=  ir(XRq,r,'r) 

/=i 

=  /r(RX)  (43) 

where  X  =  ^q,r(,r  (44) 

i=l 

The  trace  becomes  maximum  when  the  matrix  product  RX  is  symmetric  and  positive  definite. 
If  we  compute  the  SVD  of  X  as  X  =  UZVi  where  U  and  V  are  orthogonal  matrices  and  X  is 
diagonal  with  non-negative  elements,  then  /r(RX)is  maximised  if 

R  =  VUr  (45) 

To  prove  this,  we  need  to  show  that  RX  =  VU7  (UX  V7 )  is  symmetric  and  positive  definite. 

RX  =  VUr(UXVr) 

=  vxvr 

This  is  a  symmetric  matrix,  and  since  the  diagonal  elements  of  X  are  non-negative,  RX  is 
positive  definite.  Note  that  one  should  make  sure  that  the  determinant  of  R  =  VU7  is  1. 

The  OI  algorithm  in  [6]  is  implemented  as  follows: 

1.  Assume  r,  =  v,  for  i  =  1, ...,  n 

n 

2.  compute  X  =  Vqir'/ 

;=1 

3.  [U  ,X,V]  =  5vrf(X) 

4.  =  VUy  (initial  R  estimate  for  the  next  stage) 

5-  to„,=-A-1XA,Rqf 

1*1 

6.  HR^-+U fori=i'  '» 

7.  =  dt \t  for  i  =  1, ...,  n 

8.  Go  to  step  2  and  repeat  until  R  and  t  converge. 

The  vehicle  position  (in  reference  frame)  is  then  obtained  as  p  -  .  Steps  2-4  update  the 

estimate  of  R ,  which  is  then  used  to  compute  the  translation  vector  in  the  fifth  step. 

We  also  look  at  a  non-iterative  method  proposed  by  Lepetit,  et  al.  [5]  which  is  not  based  on 
the  collinearity  cost  metric.  This  method  expresses  the  3D  landmark  points  as  a  weighted  sum 
of  four  control  points  and  solves  the  problem  in  terms  of  their  coordinates.  The  computer 
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simulations  of  this  method  indicate  that  this  method  is  not  as  accurate  and  robust  as  the  Ol 
method.  For  fine-tuning,  this  method  uses  Gauss-Newton  optimisation  [1]  which  sometimes 
fails  to  converge  if  the  measurements  are  noisy  and  the  landmark  number  is  small.  Hence,  we 
exclude  this  method  in  the  performance  comparison. 


3.  Simulations 


3.1  Process  Overview 

The  simulation  process  is  outlined  in  Figure  4.  The  proposed  method  on  the  left  side  starts 
with  initial  azimuth  estimation  (AZIPE).  Optionally,  the  AZIPE  can  also  estimate  the  position 
if  necessary.  In  the  simulations,  we  start  counting  the  iterations  after  the  AZIPE.  Likewise,  the 
benchmark  method  runs  the  absolute  orientation  (AO)  to  come  up  with  the  initial  attitude 
estimates  before  the  iteration  count-up  starts. 


Known:  landmark  position  q={qi  qn) 
Initial  Guess:  <t>0=0,  0o=O,  d-^  .do}=1 
Input:  uLOS  vector  v={v1(...v„} 


Figure  4:  Process  overview  of  the  proposed  method  (left)  and  benchmark  method  (right) 


We  assume  that  the  vehicle's  tilt  angles  remain  moderately  small  (eg.,  <40q),  therefore 
assuming  zero  tilt  angles  for  the  AZIPE  may  be  acceptable.  On  the  other  hand,  the  initial  AO 
algorithm  assumes  r,  =  v/  (i.e.,  all  the  landmarks  are  unit  distance  away  from  the  vehicle).  The 
role  of  the  initial  estimator  is  to  bring  the  initial  estimate  close  to  the  global  minimum,  so  the 
global  convergence  is  ensured  when  the  AIPE  or  OI  is  executed. 


11 


DSTO-TR-2278 


The  AIPE  and  OI  were  run  iteratively,  gradually  improving  the  attitude  and  position 
estimates  by  feeding  back  the  attitude  and  vehicle/ landmark  distance  estimates.  Steps  2-4  in 
Section  2.4  are  run  to  produce  the  initial  R  estimate  required  by  the  fifth  step.  Steps  5-7 
followed  by  steps  2-4  (i.e.,  AO)  form  the  OI  process. 

The  following  are  the  default  parameter  settings  for  the  simulations.  The  parameters  remain 
unchanged  except  for  the  one  whose  effect  we  want  to  see  on  the  system  performance.  These 
are 

•  a{0el )  =  3°  and  <r(0az)  =  3°  for  each  AOA  measurements. 

•  Landmark  positions:  {(0, 0,  -10),  (40, 0,  -10),  (0, 40,  -10),  (40, 40,  -10)}  ((0, 20,  -10),  (20, 0,  - 
10),  (20,  40,  -10),  (40,  20,  -10)}m  (negative  z  =>  positive  altitude) 

•  Vehicle  orientation:  [<f>,0,y/]  =  [20°,  20°,  45°  ] 

•  Vehicle  position:  [x,y,z]  =[4m,  10m,  0m] 

•  Number  of  iterations=4. 

In  the  following  subsections,  we  conduct  1000  Monte-Carlo  simulations  to  generate  the 
performance  metric  in  terms  of  estimation  error  statistics.  It  is  expressed  as  the  root  mean 
square  (RMS)  of  the  horizontal-position,  height,  tilt  and  azimuth  errors.  We  define  the 

horizontal  position  and  tilt  errors  as  yjx^  +  y2erT  and  +9)n  respectively.  The  error  curves 
from  the  proposed  and  benchmark  methods  will  be  shown  in  green  and  blue  respectively,  and 
will  be  compared  with  the  Cramer-Rao  Lower  Bound  (CRLB)  shown  in  red. 

The  CRLB  represents  the  smallest  estimation  error  (RMS)  that  any  unbiased  method  can 
possibly  achieve  for  the  given  geometry  and  measurement  errors.  We  assume  that  the 
measurement  errors  are  Gaussian  of  mean  zero.  The  descriptive  derivation  of  the  CRLB  is 
given  in  Appendix  D. 

3.2  Convergence  Speed 

In  this  subsection,  we  examine  the  convergence  trend  of  both  localisation  methods.  The 
iteration-count  up  to  10  is  attempted  and  the  results  are  given  in  Figure  5.  Zero  iteration 
means  that  the  process  stopped  after  the  initial  stage.  For  index  of  zero,  the  horizontal  position 
error  of  the  AO  is  much  larger  than  that  of  the  AZIPE,  whereas  the  other  error  terms  from  the 
two  methods  are  comparable.  Both  methods  converge  rapidly:  3-4  iterations  for  the  OI  and  2 
iterations  for  the  AIPE,  It  appears  that  the  small  angle  approximations  in  the  AIPE  tolerate  the 
initial  roll  and  pitch  errors  of  (20°, 20°)  and  azimuth  error  of  7°  (cr) 
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Hw  Po*«ton  £rrof<m) 


8*nchm«rt 

Propo#*d 

CRLB 


5  6 

to  of  Tl*f«l*or» 


Aiimufh  £rra  (flkg) 


No  of  TI*r*tiona 


Figure  5:  Estimation  Error  versus  number  of  iterations  at  default  location  (4,10,0) 


Apart  from  the  convergence  speed,  the  AIPE  is  also  favoured  in  terms  of  post  convergence 
residual  errors.  It  is  noticed  that  the  error  differences  are  more  evident  for  angular  terms  than 
for  the  positional  terms.  Such  differences  between  the  two  methods  arise  from  the  fact  that  the 
objective  function  used  in  the  AO  (see  Equation  (40))  is  not  always  a  close  approximation  of 
true  collinearity  measure.  The  weights  are  not  equally  distributed  to  the  landmarks  -  more 
weights  are  given  to  the  far  landmarks  than  to  the  near  landmarks.  The  AO  would  perform 
better  if  the  landmarks  are  equally  distant  from  the  vehicle. 


To  demonstrate  this,  we  ran  another  simulation  where  the  vehicle  is  moved  to  (20,20,0)  m  so  that  the 
distances  to  all  the  landmarks  are  similar.  The  new  estimation  errors  are  given  in 

Figure  6.  With  the  new  geometric  arrangement,  the  AO  algorithm  produces  a  very  accurate 
initial  pose  estimate,  which  gets  slightly  worse  as  the  OI  iteration  takes  place.  The  OI  solutions 
converge  after  1-2  iterations,  and  the  attitude  accuracy  is  significantly  closer  to  that  of  the 
AIPE.  The  AIPE  converges  after  2  iterations  as  before,  showing  convergence  patterns  that  are 
more  consistent  regardless  of  the  geometric  changes.  For  the  remaining  simulations,  we  apply 
a  fixed  iteration-count  of  4  to  the  two  methods. 
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no  of  rtarahora 


Till  Error  (d*g) 


Azimuth  Error  (d*g) 


Figure  6:  Estimation  errors  versus  number  of  iterations  at  another  location  (20,  20,  0).  Initial  AO 
algorithm  of  the  benchmark  method  works  better  as  the  distances  from  the  vehicle  to  the 
landmarks  are  similar. 

3.3  Effect  of  Measurement  Errors 


Here  we  explore  the  effect  of  the  measurement  (AO A)  errors  on  the  estimation  accuracy.  The 
elevation  and  azimuth  errors  (cr)  are  equally  varied  from  0.1°  to  10°,  and  the  results  are 
shown  in  Figure  7.  The  green  curves  (proposed)  remain  closer  to  the  CRLB  curve  (red)  than 
the  blue  curves  (benchmark)  do,  within  the  entire  angle  error  range. 


It  is  noticed  that  the  green  curves  start  to  depart  from  the  red  curves  when  the  AOA  angle 
exceeds  3°.  To  explain  this  we  revisit  Equation  (4) 


|Rq,+t 

2 

( 

vr 

-1\2 

Rq, +t 

1  d, 

Vi 

L  4  JJ 

According  to  the  diagram  in  Figure  3,  we  realise  that 

=  (sin (AOA  _errff . 


Rq,+tir 

( 

\T 

Rq,  +tT 

d,  | 

V, 

V 

d,  JJ 

For  small  AOA  errors  (eg.,  <3°) ,  the  following  approximation  holds: 
sin (AoA_err)  a  AOA_err  . 
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Therefore,  for  small  AOA  errors,  the  cost  function  can  be  expressed  as 

n 

£(R,t) «  ^(AOA _err)2 ,  which  is  the  same  as  the  cost  function  associated  with  the  CRLB. 
/'=! 


Hoc  Po»<  >or  Enc>f(m)  Haight  Error  (m) 


Till  Error  (dag) 


Azimuth  Error  (dag) 


Figure  7:  Estimation  errors  versus  AOA  noise 

3.4  Effect  of  Initial  Tilt  Error 

In  Section  3.2,  we  observed  that  the  AZIPE  produced  a  large  azimuth  error  of  7°  when  the 

initial  tilt  error  was  20°.  If  the  initial  tilt  error  is  larger,  then  the  azimuth  error  will  be  even 
larger  placing  the  initial  estimate  far  from  the  global  minimum.  Here  we  examine  if  the  AIPE 
and  OI  are  capable  of  consistently  steering  the  estimate  towards  the  global  minimum. 

Figure  8  shows  no  sign  of  divergence  -  the  blue  and  red  curves  follow  the  shape  of  the  red 
curves.  The  shape  variation  of  the  curves  is  mainly  due  to  the  fact  that  the  AOA 
measurements  change  when  the  vehicle  tilt  changes.  For  example,  the  landmarks  in  the  left 

and  right  of  the  vehicle  with  zero  tilt,  will  appear  above  and  below  when  the  vehicle  rolls  90°. 
Such  measurement  changes  alter  the  cost  function  leading  to  the  variations  in  all  three  curves 
in  Figure  8.  It  appears  that  the  proposed  and  the  benchmark  methods  are  capable  of 
consistently  bringing  the  coarse  initial  estimate  to  global  minimum.  Again,  the  proposed 
method  produces  more  accurate  estimates  than  the  benchmark. 
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Hof.  Po*ttton  Efrof<m)  Height  Error  (m) 


Tilt  Error  (d#fl) 


2ft 


26 

24 


Azimuth  Error  (d*g) 

i  — i - 1 - r 

B*nchm*rk 

Propo**d 

CRLB 


40 


Figure  8  Estimation  errors  versus  the  vehicle  tilt  angles.  Tilt  angles  are  applied  equally  to  the  roll  and 
pitch. 


3.5  Effect  of  Landmark  Numbers 

In  this  subsection,  we  look  at  the  effect  of  landmark  numbers  (n)  on  the  estimation  accuracy. 
The  landmark  number  is  varied  from  n- 4  to  16  in  steps  of  2.  The  16  landmark  locations  are 
given  in  Appendix  E. 

As  in  Figure  9,  the  localisation  errors  clearly  decrease  as  n  increases.  The  error  reductions  for 
the  attitude  are  more  gradual  and  the  pattern  seems  to  continue  past  n=16.  On  the  other  hand, 
for  the  position,  the  error  reductions  are  more  abrupt  for  k=4~6,  and  become  slower  for  the 
larger  n. 

As  expected,  increasing  the  number  of  landmarks  improves  the  estimation  accuracy,  however 
this  also  increases  the  computational  effort  ( 0(n )).  Again,  the  proposed  method  outperforms 
the  benchmark  method. 
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Hof  Petition  Err  of  (m) 


B*nctim*fk 

Propo**d 

CRIB 

2 


0  6 - 1 - 1 - J - ‘ - 1 - 

4  6  8  10  12  14  16 

ofLundmwfc* 


Tilt  Error  (d*c) 


H*ight  Error  (m) 


Aiimuth  Error  (d*g) 


Numbtf  of  Landmark* 


Figure  9  Estimation  errors  versus  number  of  landmarks. 

3.6  Random  Positions  and  Orientations 

In  this  subsection,  we  randomise  the  vehicle  position/ orientation  and  the  landmark  positions. 
Both  the  vehicle  and  landmark  positions  are  uniformly  distributed  within  [-20  20]  for  all  three 

axes.  The  vehicle  roll  and  pitch  angles  are  uniformly  distributed  within  [-40°  40°]  and  the 

azimuth  angle  has  an  uniform  distribution  within  [-180°  180°].  The  aim  is  to  try  exhaustive 
combinations  of  geometries  and  see  if  the  proposed  method  is  consistently  better  than  the  OI 
method.  In  this  test,  we  try  five  levels  of  difficulties,  where  the  difficult  setting  has  a  smaller 
number  of  very  noisy  measurements  and  the  easy  setting  has  a  large  number  of  less  noisy 
measurements  (see  Table  1). 


Table  1:  Five  difficulty  indices  and  the  associated  measurement  arrangements 


Difficulty  Index 

1 

2 

3 

4 

5 

Landmark  Numbers 

8 

12 

16 

20 

24 

Measurement  Noise 

5 

4 

3 

2 

1 
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Sotting  No  Sotting  No 


Tilt  Err  Of  (bag) 


Aumuth  Errot  (bag) 


- 1 

1 

- / 
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- - 1 — . .  i  ..  .1  — - 

Banchmark 

Propoaad 
CRIB  . 

KC  : 

- f  -\-4  -^1 - 

•  V 

;  ;  x  ' 

■  “  r 1  i  -i  — 

1  T  - - T 

c  '  X' ' r  *  T  — 

: 

i_ i_ i 

1  > 

l  l  I  •  "  It » 

1  1  1 

i  * l 

)1 _ i _ I _ I _ I _ i _ i _ I _ 

1  15  2  25  3  3.5  4  45  5 


Satting  No 


Figure  1 0:  Estimation  errors  versus  difficulty  indices ,  when  the  vehicle-landmark  geometries  are  made 
random .  Higher  the  index ,  easier  the  problem. 


Figure  10  shows  the  RMS  errors  after  3000  Monte-Carlo  simulations.  All  the  errors  terms 
decrease  as  the  number  of  measurements  increases  and  the  measurement  noise  decreases.  It  is 
also  clear  that  the  proposed  method  consistently  outperforms  the  OI  method,  reinforcing  the 
previous  findings. 
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4.  Conclusions 


In  this  study,  we  present  an  accurate  and  efficient  3D  localisation  method  that  uses  the  angle 
of  arrival  measurements  from  stationary  landmarks.  The  underlying  idea  is  to  minimise  the 
collinearity  error  between  the  measured  and  estimated  uLOS  vectors.  The  algorithm  runs  in 
two  parts  -  the  AZIPE  for  initial  azimuth  estimation  assuming  zero  tilt  and  the  AIPE  for 
progressively  improving  all  three  orientation  angles.  The  findings  from  the  experiments  are: 

•  The  proposed  algorithm  converges  fast  (e.g„,  <3  iterations)  for  general  geometric 
configurations. 

•  The  proposed  algorithm  tolerates  large  measurement  and  tilt  errors. 

•  The  estimation  accuracy  improves  as  the  number  of  landmarks  increases.  However, 
the  computation  effort  also  increases  as  the  number  of  landmark  increases  (~  O(n)) . 

•  In  all  cases,  the  proposed  method  outperforms  the  benchmark  method. 

The  proposed  method  appears  to  be  accurate,  globally  convergent  and  computationally 
efficient  requiring  only  a  small  number  of  iterations. 
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Appendix  A:  Derivation  of  Optimal  Translation 


Given 


E(R,t)  =  Z 


Rq,  +t 


Rq,  +t 


If  we  let  V,  =  v,v7  then  the  optimal  translation  vector,  t ,  is  given  by 

£(R,t)  =  Z 


Rq(  + 1 

Rq,  + 1 

-v,7" 

Rq,  +t 

V,7" 

Rq,  +t 

d, 

l  4  J 

d, 

d, 

=  I4KR7  +  ‘7  )(Rq,  + O' - 4(v,7  Rq,  -  v[ t)(vl  Rq,  -  v7t) 

t=\  d \ 

=  i  JtKr’  Rq,  +  q.rRrt  +  t7  Rq,  +  t7  t  -(q,7  R7  v,  - 17  v,  )(v7Rq,  -  v,rt)} 

1  =  1 

=  q,  +  2q'  R  t  + 1  t  - (q7  R'  v(v[ Rq,  -  q 7  R  v,v' t  -  t  v,v7  Rq  + 1'  v,v7 t)j 

t=i  dt 

-£-7rK<l,  +2trRq,  +trt-(q(rRrV,Rq,  -q,7RrV,t-trV,Rq,  +t7V,t)} 

J  =  1 

=  1 4{‘?  (*  -  V,)t  +  2t7  (I  -  V,)Rq,  -q;  R7  V,Rq,  +  q,7q,} 


(Al) 


(A2) 


Let  A,  =  - — ,  then  setting  — ' —  to  zero  gives 
d,  dt 


^^=I^-{2(|-V.)t  +  2(|-V.)R  q.}=° 

J-^(I-V,)t+-jr(l-Vj)Rq,=0 
1=1  d,  dt 

(IA.)t+IA-Rf i.=° 

1=1  i=i 

and  therefore 

t^(R)  =  -A-'jAlRqj  where  A=IA, 


(A3) 
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Appendix  B:  Derivation  of  Quadratic  Cost  Function 


Rq,  + 1 

2 

f 

Vr 

Rq,+t 

d, 

v/ 

L  <*■  \) 

£(R,t)  =  £ 


Replacing  Rqf+twith  G,e  +  g,  gives: 


(Bl) 


£(e)  =  Z 
/= 1  || 

n  4 

=L- 


llG,e+g,  r 

'  vf(G,e+g,)  y 

II  d, 

d,  ) 

e7G7G,e  +  g, 

?g,  +2g,rG,e  « 

f  £c[( I  -  V,  )G,e  +  2g l  (I  -  V,  )G,e  +  g,r(I  -  V,  )g, 

L  d2 

i= 1  a, 

=  £e7G,rA(G,e  +  2g,7A,G,e  +  g,7A,g, 

=  e'  e  +  2  ^g,rA,Gf le+^g,'  A,g, 

V/-i  /  v>= i  /  /=i 


£(e)  =  e'  Me  +  2m‘  e  +  d 


(B2) 

(B3) 
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Appendix  C:  Linearised  Rotation  Matrix 


cl 

c  2 

c3 

c4 

c5 

c6 

cl 

c8 

c9 

where 


cl  =  cos0cosi//o  in  the  form  of  cl  =cl0 

+  (0  )8<f>  4*  cl,  S</> 

-  (sin  0O  cos y/Q )S0  +c\2S0 

-(cos#0  sin  y/0)<5i//  +  cl3  Si// 

c2  =  cos  0O  siny/0 

+  (0  )S<t> 

-(sin#0  sin  y/())S0 
+  (cos#0  cos  i//0)Sy/ 


c3  =  -sin#0 

+  (0  )8<f> 

-  (cos  0O  )S0 
+  (0  )6p 

c4  =  (sin  sin  0O  cos  i//0  -  cos  <f>0  sin  y/0 ) 

+  (cos^0  sin#0  cosy/0  +  sin^0  sin  y/0  )S<f) 

+  (sin  <f>0  cos  0O  cos  y/0  )S0 
+  (cos cos^/0  -sin#0  sin^0  sin y/0)St// 

c5  =  (sin^0  sin#0  sin<//0  +  cos^0  cos<//0) 

+  (cos^0  sin#0  sim//0  -sin^0  cos y/0)S# 

+  (sin^0  cos#0  sin  if/Q)S0 
+  (-cos^0  sim//0  +sin^0  sin  0O  cos y/0)Sy/ 

c6  =sin^0  cos#0 

+  (cos^0  cos#0)<5^ 

-(sin^0  sin#0)<ft9 
+  (0)<5y/ 


(Cl) 


(C2) 


(C3) 


(C4) 


(C5) 


(C6) 
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cl  =  (cos  sin  0O  cos  y/Q  +  sin  sin  i//0 ) 

+  (-sin^0  sin#0  cosy/0  +  cos^0  sin 
+  (cos  (j>Q  cos  0O  cos  y/0  )S0 
+  (sin  <j)Q  cos  y/0  -  cos  <j>0  sin  0O  sin  y/0  )5y/ 

c8  =  (cos^0  sin  0O  sin  y/0  -sin^0cosy/0) 

+  (-sin^0  sin#0  siny/0  -  cos  cosy/0)S<fi 
+  (cos  </>0  cos  0O  sin  y/0  )80 
+  (sin  (f>Q  sin  y/0  +  cos <f>Q  sin  0O  cos  y/Q  )Sy/ 

c9  =  cos^0  cos#0 

-(sin  4)  cos  0O  )8<j> 

-(cos  </>0  sin#o)£0 
+  (0)<fy/ 


(C7) 


(C8) 


(C9) 
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Appendix  D:  Derivation  of  CRLB 

The  Maximum  Likelihood  Estimator  (MLE)  [1  ][7]  has  the  following  objective  function: 


*r  \at  -/  fx) 

Minimise  £(x)  =  £ [a, -/a<(x)  /?-//,(*)]  N'1  • 

1=1  |_P,  ~  J  pt  w 

where 

\-[x  y  z  <f>  6  y/]1  :  vehicle  position  and  orientation, 

'<  0 
.  0 

r  \ 


(Dl) 


N,  = 


:  covariance  matrix  for  ith  elevation  ( a )  and  azimuth  (/?)  measurements, 


/«,(*)■ tan' 


■f  v,(2) 

v,(D 


/  /A(x)  =  tan 


~v,(3) 


\/v,(l)2  +  v,(2)2 


and  v,  =  R(p,  ~[x,y,z]' ). 


The  elevation  and  azimuth  functions  fa  (x)  and  /fl(x)  are  non-linear  (contains  trigonometric 
terms).  Omitting  (x)  from  the  functions,  we  define  the  Jacobian  matrix  for  the  ith  landmark  as 


X 

0fa, 

C, 

Wo, 

wa, 

X 

dx 

8y 

dz 

5<j> 

50 

dy/ 

% 

dfPl 

8fp, 

ofP, 

dfp, 

dfPl 

dx 

dy 

dz 

d(j> 

dd 

dy/ 

Vertically  cascading  for  all  the  landmarks,  we  get 


X 

"N, 

0 

0 

and  N  = 

0 

0 

X 

0 

0 

(D2) 


(D3) 


The  error  covariance  matrix  for  x  is  given  as 

P  =  (F'1N'IF)'1  (D4) 


And  the  square  root  of  its  diagonal  elements  are  the  CLRBs  for  the  six  parameters, 
(x,y,z,</>,0,ty) . 
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Appendix  E:  Landmark  Locations 


Total  of  16  landmarks  are  available,  and  the  first  n  landmarks  are  chosen  in  the  simulations. 
For  example,  if  n= 6  then  pi  to  are  selected.  Note  that  negative  z  means  positive  height. 


Pl 

- 

(0,  0,  -10) 

m 

P2 

= 

(40,  0,  -10) 

m 

P3 

= 

(0,  40,  -10) 

m 

P4 

= 

(40,  40,  -10) 

m 

Ps 

= 

(0,  20,  -10) 

m 

Pb 

= 

(20,  0,-10) 

m 

P7 

= 

(40,  20,  -10) 

m 

Pg 

= 

(20,  40,  -10) 

m 

P9 

= 

(0,  0,  -20) 

m 

Pio 

= 

(40,  0,  -20) 

m 

Pll 

= 

(0, 40,  -20) 

m 

P 12 

- 

(40,  40,  -20) 

m 

P 13 

= 

(0,  0,  -40) 

m 

P 14 

= 

(40,  0,  -40) 

m 

Pl5 

= 

(0,  40,  -40) 

m 

Plb 

= 

(40,  40,  -40) 

m 
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